package simulator.implementation.agent.essential;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import simulator.meta.agent.ControlledAgent;
import util.essential.SourceGetter;

public class ControlledAgentWithInteralRepresentation extends ControlledAgent{
	//Constants:
	protected final int STEP_INCREASE = 1;
	protected final int TURN_INCREASE = 50;
	protected final boolean GOING_FORTH = true;
	protected final boolean TURNNING_RIGHT = true;
	private final String PROMPT = "CtrlledAgentWIRep> ";
	
	//Attributes:
	private double azimuth;
	private int x,z;
	private Point3d[][]internalWorld;
	private int numberOfTiles;
	private boolean isBump;
	private boolean isShoot;
	
	//Constructor:
	public ControlledAgentWithInteralRepresentation(Vector3d arg0, String arg1) {
		super(arg0, arg1);
		this.azimuth = 0;
		this.internalWorld = null;
		this.x = this.z = 0;//...where x,z in [0..(numberOfTiles-1)].
		this.numberOfTiles = 0;
		this.isBump = false;
		this.isShoot = false;
	}
	
	//Methods:
	public void setAwarenessAboutDisplacementWithinTheWorld(int worldsize,int numberOfTiles){
		SourceGetter.out.debug(this.PROMPT+"[setAwarenessAboutDisplacementWithinTheWorld] ","worldsize = "+worldsize+", numberOfTiles = "+numberOfTiles);
		this.numberOfTiles = numberOfTiles;
		int size = (int)(worldsize/numberOfTiles);
		this.internalWorld = new Point3d[size][size];
		for (int z = 0; z < this.internalWorld.length; z++) {
			for (int x = 0; x < this.internalWorld[z].length; x++) {
				this.internalWorld[z][x] = new Point3d(f(x,worldsize,numberOfTiles),0,f(z,worldsize,numberOfTiles));
				SourceGetter.out.debug(this.PROMPT+"[setAwarenessAboutDisplacementWithinTheWorld] ","p("+x+","+z+") = "+this.internalWorld[z][x].toString());
			}
		}
	}
	private int f(int x,int l, int n){
		return (l*(n-(2*x)-1))/(2*n);
	}
	private boolean areIntoRange(double x,double z){
		SourceGetter.out.debug(	"("+x+">= 0 && "+x+"<"+this.numberOfTiles+" && "+
								z+">= 0 && "+z+"<"+this.numberOfTiles+")");
		return (x>=0 && x<this.numberOfTiles &&
				z>=0 && z<this.numberOfTiles);
	}
	public void stepUp(){
		int x = this.x,
			z = this.z;
		double azimut = this.fromRadiansToDegrees(this.getAzimuth()),d,h, ansFactor = 1;
		boolean useXToCalculateDiference = true;
		
		if(azimut == 360 || azimut == 0){
			//assignationRule = 0;
			useXToCalculateDiference = false;
			ansFactor = 1;
			x--;
		}else if(azimut == 180){
			useXToCalculateDiference = false;
			ansFactor = -1;
			x++;
		}else if(azimut == 90){
			useXToCalculateDiference = true;
			ansFactor = 1;
			z++;
		}else if(azimut == 270){
			useXToCalculateDiference = true;
			ansFactor = -1;
			z--;
		}
		
		SourceGetter.out.debug(this.PROMPT+"[stepUp] ","Comparing... Current position: ("+this.x+","+this.z+");" +
								" Next position: ("+x+","+z+")"+
								" --> this.areIntoRange("+x+","+z+") = "+this.areIntoRange(x,z));
		if(this.areIntoRange(x,z)){
			this.isBump = false;
			//Updating coordinates:
			this.x = x;	this.z = z;
			//Going to new position:
			Point3d	targetPosition =  this.internalWorld[z][x],currentPosition = new Point3d();
			double distance = 0, lastDistance = Double.MAX_VALUE;
			final byte MAX_SPEED = 5;
			boolean isSlopeDown = false;
			//Calculating angle to turn:
			this.getCoords(currentPosition);			
			//Calculating difference:
			d = (useXToCalculateDiference)?(currentPosition.getX() - targetPosition.getX()):(currentPosition.getZ() - targetPosition.getZ());
			//Calculating hipotenusa:
			h = currentPosition.distance(targetPosition);
			//Calculating correction angle:
			double correctionAngle = ansFactor*Math.asin(d/h);
			//Turning with correction angle:
			this.turn(correctionAngle);
			SourceGetter.out.debug(this.PROMPT+"[stepUp] ", "Pc: "+currentPosition.toString()+"; Pt: "+targetPosition.toString());
			SourceGetter.out.debug(this.PROMPT+"[stepUp] ", "\tCurrent Azth: "+azimut);
			SourceGetter.out.debug(this.PROMPT+"[stepUp] ", "\tCorrection: "+this.fromRadiansToDegrees(correctionAngle)+" (d->"+d+"); New Azth = "+(azimut+this.fromRadiansToDegrees(correctionAngle)));
			do{
				try {Thread.sleep(2);} catch (InterruptedException e) {e.printStackTrace();}
				distance = (double)(targetPosition.distance(currentPosition));
				this.setTranslationalVelocity(MAX_SPEED*distance);
				this.getCoords(currentPosition);
				isSlopeDown = (lastDistance>=distance);
				lastDistance = distance;
			}while(isSlopeDown);
			this.setTranslationalVelocity(0);
			this.turn(-correctionAngle);
		}else{
			this.isBump = true;	
		}
		SourceGetter.out.debug(this.PROMPT+"[stepUp] ", (this.isBump)?"******HIT!!!!!!!!!":"******NOT HIT.");
	}
	public void stepDown(){
		this.turnRight();
		this.turnRight();
		this.stepUp();
		this.turnLeft();
		this.turnLeft();
	}
	public void turnLeft(){
		this.turn(Math.PI/2);
	}
	public void turnRight(){
		this.turn(-1*Math.PI/2);
	}
	
	/**
	 * This method allows to rotate robot.
	 * @param rad
	 */
	private void turn(double rad){
		this.rotateY(rad);
		//Updating azimuth:
		final double DEGREE_360 = (2*Math.PI);
		double tmp = (this.azimuth + rad);
		this.azimuth = (tmp<0)?DEGREE_360+tmp:(tmp>=DEGREE_360)?tmp-DEGREE_360:tmp;
		SourceGetter.out.debug(this.PROMPT+"[turn("+this.fromRadiansToDegrees(rad)+")] ","Azimut: "+this.fromRadiansToDegrees(this.azimuth));
	}
	
	/**
	 * @param rad
	 * @return radians.
	 */
	private double fromRadiansToDegrees(double rad){
		return (rad*180)/Math.PI;
	}
	
	/**
	 * @return azimuth.
	 */
	public double getAzimuth(){
		return this.azimuth;
	}

	/**
	 * @return whether agent is either bumping or not.
	 */
	public boolean isBump() {
		return isBump;
	}

	public boolean isShoot() {
		return isShoot;
	}

	public void setShoot(boolean isShoot) {
		this.isShoot = isShoot;
	}

	public int getX() {
		return x;
	}
	
	
}
